#ifndef SAFE_ZONE_HH
#define SAFE_ZONE_HH

#include "zuc/nml_intf/zucpos.h"
typedef struct
{
    PmCartesian point;
    PmRpy rpy;
} CartesianPos;

typedef struct
{
    char name[255];
    int enable;
    int modify;
    double d;
    double safeD;
    CartesianPos planePoints[3];
    CartesianPos safePoint;
    CartesianPos norm;
} SafePlane;

typedef struct
{
    int enable;
    CartesianPos center;
    double radius;
} SafeSphere;

typedef struct
{
    int enable;
    CartesianPos diamondPoints[2];
} SafeDiamond;

typedef enum
{
    PLANE = 1,
    SPHERE,
    DIAMOND
} SafeZoneType;

typedef struct
{
    SafeZoneType type;
    int openEnable;
    int autoEnable;
    int elbow_pos_limit_enable;
    int wrist_pos_limit_enable;
    int safeZoneMode;
    int safe;  //1:安全，0，危险区域
    union
    {
        SafePlane plane[6];
        SafeSphere sphere[6];
        SafeDiamond diamond[6];
    } zone;

} SafeZone;

extern int checkSafePlane(SafePlane plane, ZucPose curPose, double* dist, int elbow_wrist_flag, double R_elbow, ZucPose current_user_offset);
extern int checkSafeSphere(SafeSphere sphere, ZucPose curPose, double* dist);
extern int checkSafeDiamond(SafeDiamond diamond, ZucPose curPose, double* dist);
extern int checkSafezone(SafeZone zone,
                         ZucPose curPose,
                         ZucPose cur_elbow_Pose,
                         int elbow_wrist_flag,
                         double R_elbow,
                         ZucPose current_user_offset,
                         int pre_flag,
                         checksafefalg_out* checkSafeflag);
extern int checkSafezone_dangerous(SafeZone zone, ZucPose curPose, ZucPose cur_elbow_Pose, int elbow_wrist_flag, double R_elbow, ZucPose current_user_offset);
extern void convertPointToWorld(const ZucPose* user_point, const ZucPose* current_user_offset, ZucPose* base_point);
#endif
